/*
    Distance Between Points Filter(s)

    Copyright (C) 2002 Robert Lipe, robertlipe@usa.net

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA

 */
#include "defs.h"
#include "filterdefs.h"
#include "grtcirc.h"

#if FILTERS_ENABLED

#ifndef M_PI
#  define M_PI 3.14159265358979323846
#endif

static route_head *cur_rte = NULL;

static double pos_dist;
static double max_diff_time;
static char *distopt = NULL;
static char *timeopt = NULL;
static char *purge_duplicates = NULL;
static int check_time;

typedef struct {
	double distance;
} extra_data;

static
arglist_t position_args[] = {
	{"distance", &distopt, "Maximum positional distance",
		NULL, ARGTYPE_FLOAT | ARGTYPE_REQUIRED, ARG_NOMINMAX },
	{"all", &purge_duplicates, 
		"Suppress all points close to other points", 
		NULL, ARGTYPE_BOOL, ARG_NOMINMAX }, 
	{"time", &timeopt, "Maximum time in seconds beetween two points",
		NULL, ARGTYPE_FLOAT | ARGTYPE_REQUIRED, ARG_NOMINMAX },
	ARG_TERMINATOR
};

static double
gc_distance(double lat1, double lon1, double lat2, double lon2)
{
	return gcdist( 
	    RAD(lat1),
	    RAD(lon1),
	    RAD(lat2),
	    RAD(lon2)
	    );
}

/* tear through a waypoint queue, processing points by distance */
static void 
position_runqueue(queue *q, int nelems, int qtype)
{
	queue * elem, * tmp;
	waypoint ** comp;
	int * qlist;
	double dist, diff_time;
	int i = 0, j, anyitem;

	comp = (waypoint **) xcalloc(nelems, sizeof(*comp));
	qlist = (int *) xcalloc(nelems, sizeof(*qlist));

	QUEUE_FOR_EACH(q, elem, tmp) {
		comp[i] = (waypoint *)elem;
		qlist[i] = 0;
		i++;
	}
	
	for (i = 0 ; i < nelems ; i++) {
		anyitem = 0;

		if (!qlist[i]) {
			for (j = i + 1 ; j < nelems ; j++) {
				if (!qlist[j]) {
					dist = gc_distance(comp[j]->latitude, 
						comp[j]->longitude, 
						comp[i]->latitude, 
						comp[i]->longitude);
		
						/* convert radians to integer feet */
					dist = (int)(5280*radtomiles(dist));
					diff_time = fabs( waypt_time(comp[i]) - waypt_time(comp[j]) );
					
					if (dist <= pos_dist) {
						if(check_time && diff_time >= max_diff_time)
							continue;
						
						qlist[j] = 1;
						switch (qtype) {
							case wptdata:
								waypt_del(comp[j]);
								waypt_free(comp[j]);
								break;
							case trkdata:
								track_del_wpt(cur_rte, comp[j]);
								break;
							case rtedata:
								route_del_wpt(cur_rte, comp[j]);
								break;
							default:
								break;
						}
						anyitem = 1;
					}
				}
			}
				
			if (anyitem && !!purge_duplicates) {
				switch (qtype) {
					case wptdata:
						waypt_del(comp[i]);
						break;
					case trkdata:
						track_del_wpt(cur_rte, comp[i]);
						break;
					case rtedata:
						route_del_wpt(cur_rte, comp[i]);
						break;
					default:
						break;
				}
				waypt_free(comp[i]);
			}
		}
	}
						
	if (comp)
		xfree(comp);
  
	if (qlist)
		xfree(qlist);
}

static void
position_process_route(const route_head * rh) { 
    int i = rh->rte_waypt_ct;

    if (i) {
    	cur_rte = (route_head *)rh;
        position_runqueue((queue *)&rh->waypoint_list, i, rtedata);
        cur_rte = NULL;
    }

}

static void 
position_noop_w(const waypoint *w)
{
}

static void 
position_noop_t(const route_head *h)
{
}

void position_process(void) 
{
	int i = waypt_count();
	
	if (i)
		position_runqueue(&waypt_head, i, wptdata);
	
	route_disp_all(position_process_route, position_noop_t, position_noop_w);
	track_disp_all(position_process_route, position_noop_t, position_noop_w);
}

void
position_init(const char *args) {
	char *fm;

	pos_dist = 0;
	max_diff_time = 0;
	check_time = 0;

	if (distopt) {
		pos_dist = strtod(distopt, &fm);

		if ((*fm == 'm') || (*fm == 'M')) {
			 /* distance is meters */
			 pos_dist *= 3.2802;
		}
	}

	if (timeopt) {
		check_time = 1;
		max_diff_time = strtod(timeopt, &fm);
	}
}

void
position_deinit(void) {
}

filter_vecs_t position_vecs = {
	position_init,
	position_process,
	position_deinit,
	NULL,
	position_args
};

#endif // FILTERS_ENABLED
